#ifndef CONTROLLER_HPP
#define CONTROLLER_HPP

#include <geometry_msgs/Twist.h>
#include "common_msg/ObjectPosition.h"
#include "common_msg/TrackedObject.h"
#include "common_utils.hpp"

class Controller
{
public:
    Controller()
    {
        setTrackDistance(1200);
        setMaxLinearX(0.4);
        setMaxAngularZ(0.4);
        setLinearP(0.0004);
        setAngularP(0.0004);
    }

    ~Controller() {}

    void setTrackDistance(int d)
    {
        SET_RANGE(d, 800, 1600);
        mTrackDistance = d;
    }

    int &trackDistance() { return mTrackDistance; }

    void setMaxLinearX(double x)
    {
        SET_RANGE(x, 0.1, 0.4);
        mMaxLinearX = x;
    }

    double &maxLinearX() { return mMaxLinearX; }

    void setMaxAngularZ(double z)
    {
        SET_RANGE(z, 0.1, 0.4);
        mMaxAngularZ = z;
    }

    double &maxAngularZ() { return mMaxAngularZ; }

    void setLinearP(double p)
    {
        SET_RANGE(p, 0.0001, 0.0005);
        mLinearP = p;
    }

    double &linearP() { return mLinearP; }

    void setAngularP(double p)
    {
        SET_RANGE(p, 0.0001, 0.0005);
        mAngularP = p;
    }

    double &angularP() { return mAngularP; }

    // +X: right  +Y: down  +Z: forward
    // cmd_vel: +X: go forward (m/s)  +Z: turn left (rad/s)
    void calcCmdVel(common_msg::TrackedObject &obj, geometry_msgs::Twist &cmd_vel)
    {
        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0;
        cmd_vel.linear.z = 0;
        cmd_vel.angular.x = 0;
        cmd_vel.angular.y = 0;
        cmd_vel.angular.z = 0;

        if (!obj.is_missing)
        {
            common_msg::ObjectPosition pos; // = obj.position;
            pos.x = -obj.position.y;
            pos.y = -obj.position.z;
            pos.z = obj.position.x;

            int dz;
            double lx, az;
            dz = pos.z - mTrackDistance;
            if (abs(dz) < 100)
            {
                lx = 0;
            }
            else
            {
                lx = (double)dz * mLinearP;
            }
            az = -(double)pos.x * mAngularP * 1000.0 / (double)pos.z;
            
            SET_RANGE(lx, -mMaxLinearX, +mMaxLinearX);
            SET_RANGE(az, -mMaxAngularZ, +mMaxAngularZ);
            cmd_vel.linear.x = lx;
            cmd_vel.angular.z = az;
        }
    }

private:
    int mTrackDistance;
    double mMaxLinearX;
    double mMaxAngularZ;
    double mLinearP;
    double mAngularP;
};

#endif